Tom or anyone who may be able to help,

I am designing a system that is to be used to inspect circuit boards with an optical camera.

I have a standard 3 axis XYZ cartesian coordinate setup for the circuit board where it's center is at the origin (0,0,0).  I am able to rotate the board about the X-axis and the Y-axis again centered about the origin (but no rotation about the Z-axis).

Just above the board, there is a small digital camera which I have X,Y,Z control over the position of the bottom of the camera facing downward. 

So, if the board were to be flat (X-Y plane) then the camera is free to move anywhere above the board and collect data at points directed by the operator.  Some components need to be inspected at an angle, thus the operator will drive to the designated location and then begin to adjust rotations of the board (again via x-axis and y-axis only).  

In order to try to make this user friendly and prevent accidental camera to board crashes, I'd like to implement a collision avoidance capability. 

Currently, I have 2 designated fixed points on the board that I continuously monitor and apply rotation matrices to with angle inputs driven by the encoders attached to an X-axis-rotation and a Y-axis-rotation motor. Essentially allowing me to create a plane using the two points.  I draw a vector from the origin (0,0,0) to the two points, take the cross product of the the two vectors to get a normal vector.  Then since the board is centered about the origin, my plane becomes a standard plane equation Ax + By + Cz = 0. Where <A,B,C> is my normal vector.

I then find the distance from the plane to the point in space representing the bottom center of the camera.

My issue here is that in order for me to detect if there is a crash, I have to continuously check to see if the distance between the point of the camera an the plane is less than a certain threshold.  However, when I reach that threshold, I send a Jog(axis,0) command.  This creates a very violent jerk that literally shakes the machine as it stops from colliding.  I don't think this is the best approach.  Is there a way to define a limit for each axis based on where the plane lays? If there is a way to look-ahead, then I could set the final destination of the move to a point on the plane insuring it doesn't collide.

Are there any functions available to define a collision or keep-out area/zone in Kmotion C environment?

Also, lets say I wanted to get as close as I can to the board which is at some angular plane.  How would I be able to move up and down or left to right by tracking the plane with a parallel offset?

I was thinking of using linear coordinated motion with a start and an end with two sets of points that ARE ON the plane.  However, this approach would not work for me.

I have ran the CoordMotioninKFLOPTest.c

It seems to only work for total step moves of 10,000 steps or less.  I need to make longer moves, perhaps 55,000 steps.  Is this feature limited to a certain # of continuos steps? 

 

SJH,

I have gone with the approach you had mentioned and devised a 4x4 matrix to transform an (X,Y,Z) point with respect to the board-plane and determined what the transformed equivalent (X',Y',Z') would be after both the new roll and pitch angles.  I have confirmed the transformed values are correct using MatLab. 

I was then going to feed that into a custom function with an initial position of the current X,Y,Z of the camera and final position of the transformed X',Y',Z' of the camera.  

I wanted to use the coordinatedMotioninKFLOP functions to achieve a linear motion, which should restrict the shortest path to be linear and parallel to the plane (this is where I'm getting my collision avoidance). 

Here is a code snippet from the thread responsible for the motion: 

int move_linear_coordinated_xyz(void)

{

  DefineCoordSystem(0,1,2,-1);

  VM = 10000;

  A = 8000;

  OpenBuf();

  x0 = ch0->Position;

  y0 = ch1-> Position;

  z0 = ch2-> Position;

  x1 = *X_final;

  y1 = *Y_final;

  z1 = *Z_final;

  DoLinear();

  ExecBuf();

  while(!CheckDoneBuf());

}

Here is the code snippet from the thread responsible for tracking the transforms:

void Dynamic_Board_Tracking(void)

{

  

//various custom functions called here to perform the rotation based on X,Y,Z inputs with respect to the board

  *X_final = (int)A.transformed[0]; // set *X_final pointer to transformed X position

  *Y_final = (int)A.transformed[1]; // set *Y_final pointer to transformed Y position

  *Z_final = (int)A.transformed[2]; // set *Z_final pointer to transformed Z position

   move_linear_coordinated_xyz();

}

Granted these are a bit simplified to get the point across. And in my mind, this should work.

It doesn't. I don't think it is anything to do with the logic however.

For some reason, when I run it, it seems like the execution of the coordinatedMotioninKFLOP is behind one cycle.  Meaning, I can execute one set of transformed potions as a final position (x1,y1,z1) with an initial position (x0,y0,z0) as the CURRENT position as acquired from the encoders but nothing happens.  As soon as I send it another set of newly transformed final positions, it will RUN the previous point.  Furthermore, it does 2 axis coordinated rather than 3.  

Any ideas?

I'm running 4.33

I've ran the CoordinatedMotioninKFLOP Functions from both the 4.33 and 4.34 versions.  The functions from 4.33 won't allow me to input final positions larger than 10K steps (which I found out by trial and error).  The 4.34 functions run well, but I get the above mentioned delayed motion execution. 

Regards, 

 

Tom and SJH,

I finally figured out why the Coordinated Motion didn't work. Although  I was using the updated (4.34a) CoordMotionInKFLOPFunctions.c definitions, I failed to include (copy into my personal project folder) the new KMotionDef.h from the 4.34a update.  As expected, the coordinated motion now works perfectly! Super hyped about that.

Now on to my next problem (and of-course there would be).

Prior to using any CM, I had various functions that I used to move each axis using Move() commands.

In my Axes Initialization functions, I had my X-axis split between 2 axes, Axis0 & Axis6.  Where Axis6 is a slave to Axis0. Both tracked each other and worked as expected for my application.

Now with the new CM approach, I have lost my link between Axis0 & Axis6.  Hmmmm, how do I get that functionality back? 

Thanks in advance! 

 

Gentlemen,

I have finally gotten all 6 axes to coordinate their motion in harmony! Very excited! 

Tom, I am still unable to set ch6 as a slave to ch0, however, I have remedied it for now by simply adding ch6 to the CM system definition and assigning the move-to position of ch6 the same as ch0.  That seems to give me the desired motion I needed.

So for now, I have point tracking, collision avoidance and the super cool-factor of all the motion happening in synch! Annnnnnnd now my new hurdle, you knew it was coming.

Before CM, I had individual axis control for jogging.  Based on user/operator input via a touch screen UI, I would execute a simple MoveAtVel() for the desired axis at a slow vel and send the axis to it’s extreme limit (assures that I’d get a smooth stop at the limit vs Jog() cmd).  If at any point along the path the jog button was depressed, then I send a stop command to that axis. Simple and worked great.

Now I want to implement the same feature with the new CM system.  I think the same logic will work.  Send the camera to the extreme point of the desired axis and stop anywhere along the way.  I have implemented this and it does work, buuuuuuut it does NOT stop.

I have attempted the following commands while waiting for the coordinated motion buffer to be executed:

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)  //condition for button depressed

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      break;

    }

  }

This does not stop it. In fact, it attempts to stop then resumes after a short hiccup.  I have also tried the following:

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      ClearStopImmediately();

      break;

    }

  }

This is a very violent stop and I cannot get it to resume any other motion after that.

Is there an easier way to implement a similar task like the Feed Hold button in Kmotion but clear the current buffer so that when it resumes I get to jog in a different axes without continuing the old path? Hope that makes sense. 

Or perhaps something like this: 

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      while(CheckDoneBuf() != 1)   <------ I don't know what condition to wait for to insure motion has stopped

{

   //wait for CM to stop

        }

      ClearStopImmediately();

      break;

    }

  }

Thanks in advance guys! 

 

Gentlemen,

I have finally gotten all 6 axes to coordinate their motion in harmony! Very excited! 

Tom, I am still unable to set ch6 as a slave to ch0, however, I have remedied it for now by simply adding ch6 to the CM system definition and assigning the move-to position of ch6 the same as ch0.  That seems to give me the desired motion I needed.

So for now, I have point tracking, collision avoidance and the super cool-factor of all the motion happening in synch! Annnnnnnd now my new hurdle, you knew it was coming.

Before CM, I had individual axis control for jogging.  Based on user/operator input via a touch screen UI, I would execute a simple MoveAtVel() for the desired axis at a slow vel and send the axis to it’s extreme limit (assures that I’d get a smooth stop at the limit vs Jog() cmd).  If at any point along the path the jog button was depressed, then I send a stop command to that axis. Simple and worked great.

Now I want to implement the same feature with the new CM system.  I think the same logic will work.  Send the camera to the extreme point of the desired axis and stop anywhere along the way.  I have implemented this and it does work, buuuuuuut it does NOT stop.

I have attempted the following commands while waiting for the coordinated motion buffer to be executed:

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)  //condition for button depressed

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      break;

    }

  }

This does not stop it. In fact, it attempts to stop then resumes after a short hiccup.  I have also tried the following:

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      ClearStopImmediately();

      break;

    }

  }

This is a very violent stop and I cannot get it to resume any other motion after that.

Is there an easier way to implement a similar task like the Feed Hold button in Kmotion but clear the current buffer so that when it resumes I get to jog in a different axes without continuing the old path? Hope that makes sense. 

Or perhaps something like this: 

  while(CheckDoneBuf() != 1)

  {

    if(*jogVelocity == 0)

    {

      CS0_StoppingState = 0;

      StopCoordinatedMotion();

      while(CheckDoneBuf() != 1)   <------ I don't know what condition to wait for to insure motion has stopped

{

   //wait for CM to stop

        }

      ClearStopImmediately();

      break;

    }

  }

Thanks in advance guys!